#include "enc_hw_intf.h"
#include "bsp/bsp_drivers.h"
#include "utils/logger.h"
#include "os/os.h"
#if CONFIG_USE_MAG_ENCODER
enc_pwm_t enc_pwm_;
/* ABI + PWM */
void enc_hw_intf_init(init_pos_callback cb, void *ctx) {
	enc_pwm_.cb = cb;
	enc_pwm_.ctx = ctx;
	encoder_abi_timer_init(CONFIG_MAG_ENCOCDER_MAX_RES);
	encoder_pwm_timer_init();
}

void enc_pwm_irq_handler(float time, float period){
	enc_pwm_.pwm_duty = mag_encoder_duty(time, period);
	if (enc_pwm_.pwm_duty > 1.0f) {
		if (enc_pwm_.pwm_duty > 1.01f) {
			enc_pwm_.duty_errors ++;
		}
		enc_pwm_.pwm_duty = 1.0f;
	}
	enc_pwm_.pwm_pulse_cnt = pulse_cnt_from_duty(enc_pwm_.pwm_duty);
	if (enc_pwm_.cb) {
		enc_pwm_.cb(enc_pwm_.ctx, enc_pwm_.pwm_pulse_cnt);
	}
}

void enc_hw_intf_log(void) {
	sys_debug("enc ll: duty err %d, %f, %f\n", enc_pwm_.duty_errors, enc_pwm_.pwm_duty, enc_pwm_.pwm_pulse_cnt);
}
#endif

